package pulpfizz.physics;

import java.util.ArrayList;
import java.util.Set;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;
import org.jbox2d.dynamics.World;
import org.jbox2d.dynamics.joints.DistanceJoint;
import org.jbox2d.dynamics.joints.DistanceJointDef;
import org.jbox2d.dynamics.joints.GearJoint;
import org.jbox2d.dynamics.joints.GearJointDef;
import org.jbox2d.dynamics.joints.Joint;
import org.jbox2d.dynamics.joints.JointType;
import org.jbox2d.dynamics.joints.PrismaticJoint;
import org.jbox2d.dynamics.joints.PrismaticJointDef;
import org.jbox2d.dynamics.joints.PulleyJoint;
import org.jbox2d.dynamics.joints.PulleyJointDef;
import org.jbox2d.dynamics.joints.RevoluteJoint;
import org.jbox2d.dynamics.joints.RevoluteJointDef;


public class JointUtils
{
	/**
	 * Create a distance (stick) joint between two bodies
	 * that holds the specified points at a constant distance.
	 * <BR><BR>
	 * Once the distance joint is created, it may be turned into
	 * a "soft" distance joint by using DistanceJoint::setFrequencyHz(float)
	 * to set the frequency to a non-zero value, and using 
	 * DistanceJoint::setDampingRatio(float) to tune the damping constant.
	 * <BR><BR>
	 * Distance joints do not support joint limits or motors.
	 * @param a First body
	 * @param b Second body
	 * @param xa x component of anchor point on first body
	 * @param ya y component of anchor point on first body
	 * @param xb x component of anchor point on second body
	 * @param yb y component of anchor point on second body
	 * @return Newly created DistanceJoint
	 */
	public static DistanceJoint createDistanceJoint(Body a, Body b, float xa, float ya, float xb, float yb) {
		Vec2 va = new Vec2(xa,ya);
		Vec2 vb = new Vec2(xb,yb);
		DistanceJointDef jd = new DistanceJointDef();
		jd.initialize(a, b, va, vb);
		return (DistanceJoint)a.getWorld().createJoint(jd);
	}
	
	/**
	 * Creates a distance joint between bodies that touch the given points,
	 * anchored at those points. Returns null if there are not two different
	 * bodies at the points given. Behavior is undetermined if more than one
	 * body touches a point.
	 * 
	 * @param w World to add joint to and read bodies from
	 * @param pA First point to attach to
	 * @param pB Second point to attach to
	 * @return Joint created, or null if impossible to create the joint
	 */
	public static DistanceJoint createDistanceJoint(World w, Vec2 pA, Vec2 pB)
	{
		Body[] bodies = BodyUtils.getTwoDistinctBodiesAt(w, pA, pB);
		if (bodies == null || bodies.length != 2)
			return null;
		DistanceJointDef jd = new DistanceJointDef();
		jd.initialize(bodies[0], bodies[1], pA, pB);
		return (DistanceJoint) w.createJoint(jd);
	}
	
	/**
	 * Creates and returns a simple distance joint between two bodies, based on
	 * their world body positions (NOT their centers of mass).
	 * 
	 * @param a
	 * @param b
	 * @return a new DistanceJoint
	 */
	public static DistanceJoint createDistanceJoint(Body a, Body b)
	{
		DistanceJointDef jd = new DistanceJointDef();
		jd.body1 = a;
		jd.body2 = b;
		/*
		 * Auto-length based on world body positions.
		 */
		float length = a.getPosition().subLocal(b.getPosition()).length();
		jd.length = length;

		/*
		 * Note to self: Local anchors are already set to (0,0) by the constructor.
		 */
		DistanceJoint j = (DistanceJoint) a.getWorld().createJoint(jd);
		return j;
	}
	
	
	/**
	 * Create a prismatic (piston) joint between two bodies
	 * that allows movement in the given direction.
	 * <BR><BR>
	 * dirX and dirY are given in world coordinates, scaling does not matter.
	 * <BR><BR>
	 * Joint limits and motors may be set once the joint is created.
	 * @param a First body
	 * @param b Second body
	 * @param dirX x component of allowed movement direction
	 * @param dirY y component of allowed movement direction
	 * @return Newly created PrismaticJoint
	 */
	public static PrismaticJoint createPrismaticJoint(Body a, Body b, float dirX, float dirY) {
		Vec2 dir = new Vec2(dirX, dirY);
		dir.normalize();
		PrismaticJointDef pjd = new PrismaticJointDef();
		pjd.initialize(a, b, (a.getWorldCenter().add(b.getWorldCenter())).mul(0.5f), dir);
		return (PrismaticJoint)a.getWorld().createJoint(pjd);
	}
	
	/**
	 * Create a prismatic (piston) joint between two bodies
	 * that allows movement in the given direction.
	 * <BR><BR>
	 * dir is given in world coordinates, scaling does not matter.
	 * <BR><BR>
	 * Joint limits and motors may be set once the joint is created.
	 * @param a First body
	 * @param b Second body
	 * @param dir The direction of allowed motion
	 * @return Newly created PrismaticJoint
	 */
	public static PrismaticJoint createPrismaticJoint(Body a, Body b, Vec2 dir) {
		return JointUtils.createPrismaticJoint(a,b,dir.x,dir.y);
	}
	
	/**
	 * Create a pulley joint between the two bodies.
	 * The pulley joint is connected to two bodies and two fixed ground points.
     * The pulley supports a ratio such that:
     * length1 + ratio * length2 = constant
     * Yes, the force transmitted is scaled by the ratio.
     * <BR><BR>
     * The ground anchors are the points where the "rope" touches the pulley,
     * and the anchors are the points on the bodies where the rope is attached.
     * <BR><BR>
     * Joint limits may be set after the joint is created.
	 * @param a First body
	 * @param b Second body
	 * @param groundAnchorAx x coordinate of (fixed) ground anchor for body a
	 * @param groundAnchorAy y coordinate of (fixed) ground anchor for body a
	 * @param groundAnchorBx x coordinate of (fixed) ground anchor for body b
	 * @param groundAnchorBy y coordinate of (fixed) ground anchor for body b
	 * @param anchorAx x coordinate of body anchor for body a
	 * @param anchorAy y coordinate of body anchor for body a
	 * @param anchorBx x coordinate of body anchor for body b
	 * @param anchorBy y coordinate of body anchor for body b
	 * @param ratio "Block and tackle" ratio
	 * @return Newly created PulleyJoint
	 */
	public static PulleyJoint createPulleyJoint(Body a, Body b, float groundAnchorAx, float groundAnchorAy,
														 float groundAnchorBx, float groundAnchorBy,
														 float anchorAx, float anchorAy,
														 float anchorBx, float anchorBy,
														 float ratio) {
		Vec2 gA = new Vec2(groundAnchorAx,groundAnchorAy);
		Vec2 gB = new Vec2(groundAnchorBx,groundAnchorBy);
		Vec2 aA = new Vec2(anchorAx,anchorAy);
		Vec2 aB = new Vec2(anchorBx,anchorBy);
		PulleyJointDef pjd = new PulleyJointDef();
		pjd.initialize(a, b, gA, gB, aA, aB, ratio);
		return (PulleyJoint)a.getWorld().createJoint(pjd);
	}
	
	/**
	 * Create a pulley joint between the two bodies.
	 * The pulley joint is connected to two bodies and two fixed ground points.
     * The pulley supports a ratio such that:
     * length1 + ratio * length2 = constant
     * Yes, the force transmitted is scaled by the ratio.
     * <BR><BR>
     * The ground anchors are the points where the "rope" touches the pulley,
     * and the anchors are the points on the bodies where the rope is attached.
     * <BR><BR>
     * Joint limits may be set after the joint is created.
	 * @param a First body
	 * @param b Second body
	 * @param groundAnchorA (fixed) ground anchor for body a
	 * @param groundAnchorB (fixed) ground anchor for body b
	 * @param anchorA body anchor for body a
	 * @param anchorB body anchor for body b
	 * @param ratio "Block and tackle" ratio
	 * @return Newly created PulleyJoint
	 */
	public static PulleyJoint createPulleyJoint(Body a, Body b, Vec2 groundAnchorA, Vec2 groundAnchorB,
			 Vec2 anchorA, Vec2 anchorB, float ratio) {
		return JointUtils.createPulleyJoint(a, b, 
				groundAnchorA.x, groundAnchorA.y, 
				groundAnchorB.x, groundAnchorB.y, 
				anchorA.x, anchorA.y, anchorB.x, anchorB.y, ratio);
	}
	
	/**
	 * Create a gear joint, which binds together two existing
	 * revolute or prismatic joints (any combination will work).
	 * The provided joints must attach a dynamic body to a static body.
	 * <BR><BR>
	 * A gear joint is used to connect two joints together. Either joint
	 * can be a revolute or prismatic joint. You specify a gear ratio
	 * to bind the motions together:
	 * coordinate1 + ratio * coordinate2 = constant
	 * The ratio can be negative or positive. If one joint is a revolute joint
	 * and the other joint is a prismatic joint, then the ratio will have units
	 * of length or units of 1/length.
	 * <BR><em>Warning</em>: The revolute and prismatic joints must be attached to
	 * fixed bodies (which must be body1 on those joints).
	 * @param pj1 First joint (revolute or prismatic)
	 * @param pj2 Second joint (revolute or prismatic)
	 * @param ratio Gear ratio
	 * @return Newly created GearJoint
	 */
	public static GearJoint createGearJoint(Joint pj1, Joint pj2, float ratio) {
		if ( !(pj1.getType() == JointType.REVOLUTE_JOINT || pj1.getType() == JointType.PRISMATIC_JOINT) ) {
			throw new IllegalArgumentException("Gear joints can only be created between combinations of revolute and prismatic joints.");
		} else if ( !(pj1.getType() == JointType.REVOLUTE_JOINT || pj1.getType() == JointType.PRISMATIC_JOINT)) {
			throw new IllegalArgumentException("Gear joints can only be created between combinations of revolute and prismatic joints.");
		}
		GearJointDef gjd = new GearJointDef();
		gjd.joint1 = pj1;
		gjd.joint2 = pj2;
		gjd.ratio = ratio;
		return (GearJoint)pj1.m_body1.getWorld().createJoint(gjd);
	}
	
	
	



	/**
	 * Creates a RevoluteJoint between two bodies that rotates around a given
	 * point in World coordinates.
	 * 
	 * @param a
	 * @param b
	 * @param worldCenter
	 * @return
	 */
	public static RevoluteJoint createRevoluteJoint(Body a, Body b, Vec2 worldCenter)
	{
		RevoluteJointDef jd = new RevoluteJointDef();
		jd.body1 = a;
		jd.body2 = b;

		jd.localAnchor1 = a.getLocalPoint(worldCenter);
		jd.localAnchor2 = b.getLocalPoint(worldCenter);

		RevoluteJoint j = (RevoluteJoint) a.getWorld().createJoint(jd);
		return j;
	}
	
	/**
	 * Create a revolute (pin) joint between the two bodies
	 * at the given position.
	 * <BR><BR>
	 * Joint limits and motors may be set once the joint is created.
	 * @param a First body
	 * @param b Second body
	 * @param x x coordinate of pin joint location
	 * @param y y coordinate of pin joint location
	 * @return Newly created RevoluteJoint
	 */
	public static RevoluteJoint createRevoluteJoint(Body a, Body b, float x, float y) {
		Vec2 v = new Vec2(x,y);
		return JointUtils.createRevoluteJoint(a, b, v);
	}
	
	/**
	 * Create a revolute (pin) joint between two bodies that overlap
	 * a given position.  No guarantees are made about which bodies will
	 * be chosen if more than two overlap, and if no pair of bodies can
	 * be found, nothing is done.
	 * @param w
	 * @param v
	 * @return
	 */
	public static RevoluteJoint createRevoluteJoint(World w, Vec2 v) {
		Body[] bs = BodyUtils.getTwoDistinctBodiesAt(w, v, v);
		if (bs == null || bs.length != 2) return null;
		return createRevoluteJoint(bs[0],bs[1],v);
	}


}
